#include "engine.h"
#include "internal_types.h"
#include "mathlib.h"

#include <glm\gtc\matrix_transform.hpp>
#include <glm\gtc\type_ptr.hpp>

namespace HW
{

void _i_UpdateToMatrix ( GameObject::_i_Transform& transform );
void _i_UpdateFromMatrix ( GameObject::_i_Transform& transform );

void SetEntityMatrix(TEntity entity, const TVec16 &matrix)
{
	((GameObject::_i_Entity*)(entity))->transform.mat = matrix;
	_i_UpdateFromMatrix ( ((GameObject::_i_Entity*)(entity))->transform );
}

TVec16 GetEntityMatrix(TEntity entity)
{
	return ((GameObject::_i_Entity*)(entity))->transform.mat;
}

void ScaleEntity(TEntity entity, const TVec3 &scale)
{
	// TODO: check wether it should be oldScale*newScale or newScale;
	((GameObject::_i_Entity*)(entity))->transform.scale = scale;
	_i_UpdateToMatrix ( ((GameObject::_i_Entity*)(entity))->transform );
}

void MoveEntity(TEntity entity, const TVec3 &direction, int global)
{
	if ( global )
		((GameObject::_i_Entity*)(entity))->transform.position += direction;
	else
	{
		glm::vec4 localMove = glm::make_mat4x4(((GameObject::_i_Entity*)(entity))->transform.mat.a) * glm::vec4(glm::make_vec3(direction.a),0.0f);

		((GameObject::_i_Entity*)(entity))->transform.position.X += localMove.x;
		((GameObject::_i_Entity*)(entity))->transform.position.Y += localMove.y;
		((GameObject::_i_Entity*)(entity))->transform.position.Z += localMove.z;
	}
	_i_UpdateToMatrix ( ((GameObject::_i_Entity*)(entity))->transform );
}

void TurnEntity(TEntity entity, const TVec3 &angle, int global);

void PositionEntity(TEntity entity, const TVec3 &position, int global)
{
	if ( global && GetParent(entity) )
		((GameObject::_i_Entity*)(entity))->transform.position = position - ((GameObject::_i_Entity*)(GetParent(entity)))->transform.position;
	else
		((GameObject::_i_Entity*)(entity))->transform.position = position;
	_i_UpdateToMatrix ( ((GameObject::_i_Entity*)(entity))->transform );
}

void RotateEntity(TEntity entity, const TVec3 &angle, int global)
{
	if ( global )
		((GameObject::_i_Entity*)(entity))->transform.rotation = angle;
	else
	{
		//TODO: rotate the entity in relation to the parent entity here.
	}
	_i_UpdateToMatrix ( ((GameObject::_i_Entity*)(entity))->transform );
}

TVec3 EntityRotation(TEntity entity, int global)
{
	if ( global )
		{}//TODO: calculate the entity rotation in relation to the parent entity here.
	else
		return ((GameObject::_i_Entity*)(entity))->transform.rotation;
}

TVec3 EntityPosition(TEntity entity, int global)
{
	if ( global && GetParent(entity) )
		return EntityPosition ( entity , 0 ) + EntityPosition ( GetParent(entity) , 1 );
	else
		return ((GameObject::_i_Entity*)(entity))->transform.position;
}

TVec3 EntityScale(TEntity entity)
{
	return ((GameObject::_i_Entity*)(entity))->transform.scale;
}

TVec4 EntityQuat(TEntity entity)
{
	//TODO:
	return Vec4 ( 0.0f );
}

flt EntityDistance(TEntity entity1, TEntity entity2)
{
	TVec3 dpos = EntityPosition ( entity1 , 1 ) - EntityPosition ( entity2 , 1 );

	return sqrt ( dpos.X*dpos.X +
				dpos.Y*dpos.Y +
				dpos.Z*dpos.Z );
}

void TranslateEntity(TEntity entity, const TVec3 &position, int global)
{
	//TODO:
}

void AlignToVector(TEntity entity, const TVec3 &vector, int axis, flt rate, flt roll)
{
	//TODO:
}

void PointEntity(TEntity entity1, TEntity entity2, int axis, flt rate, flt roll)
{
	//TODO:
}

void _i_UpdateToMatrix ( GameObject::_i_Transform& transform )
{
	glm::mat4 matrix = glm::mat4(1.0f);
	matrix=glm::translate(matrix,glm::make_vec3(transform.position.a));
	matrix=glm::rotate(matrix,-transform.rotation.Y,glm::vec3(0.0f,1.0f,0.0f));
	matrix=glm::rotate(matrix,+transform.rotation.X,glm::vec3(1.0f,0.0f,0.0f));
	matrix=glm::rotate(matrix,+transform.rotation.Z,glm::vec3(0.0f,0.0f,1.0f));
	matrix=glm::scale(matrix,glm::make_vec3(transform.scale.a));

	memcpy ( transform.mat.a , glm::value_ptr(matrix) , 16*sizeof(flt) );
}

void _i_UpdateFromMatrix ( GameObject::_i_Transform& transform )
{
	//TODO: calculate new pos and rot and scale here. also aabb should be updated here.
}

}